/// @NOTE: this file is included from Matrix4x4.hpp, IN cvMath:: namespace.

template<typename T> Mat4x4<T>::Mat4x4()	{}	/* cheap as possible */

template<typename T> Mat4x4<T>::Mat4x4(const Mat4x4<T> &copy)
{
	for(size_t i = 0; i < 16; ++i)
	{
		mat[i] = copy.mat[i];
	}
}

template<typename T> Mat4x4<T>& Mat4x4<T>::operator=	(const Mat4x4<T> &copy)
{
	if(this == &copy)	{ return *this; }	/* do nothing */
	return *(new (this) Mat4x4<T>(copy));	/* trick compiler to reconstruct this object, without assigment */
}

template<typename T> Mat4x4<T>& operator+=	(Mat4x4<T> &left, const Mat4x4<T> &right)
{
	for(size_t i = 0; i < 16; ++i)	{ left.mat[i] += right.mat[i]; }
	return left;
}


template<typename T> Mat4x4<T>& operator-=	(Mat4x4<T> &left, const Mat4x4<T> &right)
{
	for(size_t i = 0; i < 16; ++i)	{ left.mat[i] -= right.mat[i]; }
	return left;
}

// Full unrolled inplace mat4x4 multiplication, with few as possible scalar multiplications and temporary variables.
// TODO: re-roll it to (12) small loops and keep identity positions (4) as is. (so compiler may turn the loops into SIMD instructions)
template<typename T> Mat4x4<T>& operator*=	(Mat4x4<T> &left, const Mat4x4<T> &right)
{
	// 58 multiplications, 48 sums, 32 assigments and 22 temporary variables.
	static Mat4x4<T> ret;
	const T	A = left.mat[1] * right.mat[4],
			B = left.mat[2] * right.mat[8],
			C = left.mat[3] * right.mat[12],
			D = left.mat[6] * right.mat[9],
			E = left.mat[7] * right.mat[13],
			F = left.mat[11] * right.mat[14];
	// 	X-colum..
	ret.mat[0] =	left.mat[0] * right.mat[0] + A + B + C;
	ret.mat[1] =	left.mat[0] * right.mat[1] + left.mat[1] * right.mat[5] + left.mat[2] * right.mat[9] + left.mat[3] * right.mat[13];
	ret.mat[2] =	left.mat[0] * right.mat[2] + left.mat[1] * right.mat[6] + left.mat[2] * right.mat[10] + left.mat[3] * right.mat[14];
	ret.mat[3] =	left.mat[0] * right.mat[3] + left.mat[1] * right.mat[7] + left.mat[2] * right.mat[11] + left.mat[3] * right.mat[15];
	// Y-colum..
	ret.mat[4] =	left.mat[4] * right.mat[0] + left.mat[5] * right.mat[4] + left.mat[6] * right.mat[8] + left.mat[7] * right.mat[12];
	ret.mat[5] =	A + left.mat[5] * right.mat[5] + D + E;
	ret.mat[6] =	left.mat[4] * right.mat[2] + left.mat[5] * right.mat[6] + left.mat[6] * right.mat[10] + left.mat[7] * right.mat[14];
	ret.mat[7] =	left.mat[4] * right.mat[3] + left.mat[5] * right.mat[7] + left.mat[6] * right.mat[11] + left.mat[7] * right.mat[15];
	// Z-colum..
	ret.mat[8] =	left.mat[8] * right.mat[0] + left.mat[9] * right.mat[4] + left.mat[10] * right.mat[8] + left.mat[11] * right.mat[12];
	ret.mat[9] =	left.mat[8] * right.mat[1] + left.mat[9] * right.mat[5] + left.mat[10] * right.mat[9] + left.mat[11] * right.mat[13];
	ret.mat[10] =	B + D + left.mat[10] * right.mat[10] + F;
	ret.mat[11] =	left.mat[8] * right.mat[3] + left.mat[9] * right.mat[7] + left.mat[10] * right.mat[11] + left.mat[11] * right.mat[15];
	// W-colum..
	ret.mat[12] =	left.mat[12] * right.mat[0] + left.mat[13] * right.mat[4] + left.mat[14] * right.mat[8] + left.mat[15] * right.mat[12];
	ret.mat[13] =	left.mat[12] * right.mat[1] + left.mat[13] * right.mat[5] + left.mat[14] * right.mat[9] + left.mat[15] * right.mat[13];
	ret.mat[14] =	left.mat[12] * right.mat[2] + left.mat[13] * right.mat[6] + left.mat[14] * right.mat[10] + left.mat[15] * right.mat[14];
	ret.mat[15] =	C + E + F + left.mat[15] * right.mat[15];
	// Assing..
	for(size_t i = 0; i < 16; ++i)	{ left.mat[i] = ret.mat[i]; }
	// done. :)
	return left;
}

template<typename T> Mat4x4<T>& operator*=	(Mat4x4<T> &left, const T &right)
{
	for(size_t i = 0; i < 16; ++i)	{ left.mat[i] *= right; }
	return left;
}

template<typename T> Mat4x4<T>& identity(Mat4x4<T>& m)
{
	m.mat[0] = m.mat[5] = m.mat[10] = m.mat[15] = 1;
	m.mat[1] = m.mat[2] = m.mat[3] = m.mat[4]
	= m.mat[6] = m.mat[7] = m.mat[8] = m.mat[9]
	= m.mat[11] = m.mat[12] = m.mat[13] = m.mat[14] = 0;
	return m;
}

template<typename T> Mat4x4<T> identity()
{
	Mat4x4<T> m;
	return identity(m);
}

template<typename T> Mat4x4<T>& translationXY(Mat4x4<T> &left, const Vec2<T> &p)
{	/* form 2D-translation matrix */
	left.mat[12] = p.x;
	left.mat[13] = p.y;
	left.mat[14] = 0;
	return left;
}

template<typename T> Mat4x4<T> translationXY(const Vec2<T> &p)
{	/* build 2D-translation matrix */
	Mat4x4<T> r;
	r.mat[0]	= r.mat[5] = r.mat[10] = r.mat[15] = 1;
	r.mat[1]	= r.mat[2] = r.mat[3] = r.mat[4] = r.mat[6] = r.mat[7] = r.mat[8] = r.mat[9] = r.mat[11] = r.mat[14] = 0;
	r.mat[12]	= p.x;
	r.mat[13]	= p.y;
	return r;
}


template<typename T> Mat4x4<T>& rotationZ(Mat4x4<T> &left, const Quat2<T> &q)
{	/* form 2D-rotation matrix */
	const T z2 = q.z * q.z, wz = q.w * q.z;
	left.mat[0] = 1.0f - 2.0f * z2;
	left.mat[1] = -2.0f * wz;
	left.mat[4] = 2.0f * wz;
	left.mat[5] = 1.0f - 2.0f * z2;
	return left;
}

template<typename T> Mat4x4<T> rotationZ(const Quat2<T> &q)
{	/* build 2D-rotation matrix */
	Mat4x4<T> r;
	r.mat[10] = r.mat[15] = 1;
	r.mat[1] = r.mat[3] = r.mat[6] = r.mat[7] = r.mat[8]
	= r.mat[9] = r.mat[11] = r.mat[12] = r.mat[13] = r.mat[14] = 0;
	return rotationZ<T>(r, q);
}

template<typename T> Mat4x4<T>& rotationZ(Mat4x4<T> &left, const T &angle)
{	/* form 2D-rotation matrix */
	const T r = angle * DEGREES_TO_RAD;
	left.mat[0] = cos(r);
	left.mat[1] = sin(r);
	left.mat[4] = -left.mat[1];
	left.mat[5] = left.mat[0];
	return left;
}

template<typename T> Mat4x4<T> rotationZ(const T &angle)
{	/* build 2D-rotation matrix */
	Mat4x4<T> m;
	m.mat[10] = m.mat[15] = 1;
	m.mat[1] = m.mat[3] = m.mat[6] = m.mat[7] = m.mat[8]
	= m.mat[9] = m.mat[11] = m.mat[12] = m.mat[13] = m.mat[14] = 0;
	return rotationZ<T>(m, angle);
}

template<typename T> Mat4x4<T>& scaleXY(Mat4x4<T> &m, const T &x, const T &y)
{	// form XY scale
	m.mat[0] *= x;
	m.mat[5] *= y;
	m.mat[10] = m.mat[15] = 1;
	return  m;
}

template<typename T> Mat4x4<T> scaleXY(const T &x, const T &y)
{	// build XY scale matrix
	Mat4x4<T> m;
	m.mat[0] = x;
	m.mat[5] = y;
	m.mat[10] = m.mat[15] = 1;
	m.mat[1]	= m.mat[2] = m.mat[3] = m.mat[4]
	= m.mat[6] = m.mat[7] = m.mat[8] = m.mat[9]
	= m.mat[11] = m.mat[12] = m.mat[13] = m.mat[14] = 0;
	return m;
}

template<typename T> Mat4x4<T>& ortho(Mat4x4<T>& m, const T &left, const T &right, const T &bottom, const T &top, const T &vnear, const T &vfar)
{	// form orthographical matrix
	m.mat[0]	= 2.0 / (right - left);
	m.mat[5]	= 2.0 / (top - bottom);
	m.mat[10]	= -2.0 / (vfar - vnear);
	m.mat[12]	= -((right + left) / (right - left));
	m.mat[13]	= -((top + bottom) / (top - bottom));
	m.mat[14]	= -((vfar + vnear) / (vfar - vnear));
	m.mat[15]	= 1.0;
	return m;
}

template<typename T> Mat4x4<T> ortho(const T &left, const T &right, const T &bottom, const T &top, const T &vnear, const T &vfar)
{	// build orthographical matrix
	Mat4x4<T> m;
	m.mat[1] = m.mat[2] = m.mat[3]
	= m.mat[4] = m.mat[6] = m.mat[7]
	= m.mat[8] = m.mat[9] = m.mat[11] = 0;
	return ortho<T>(m, left, right, bottom, top, vnear, vfar);
}

template<typename T> Mat4x4<T>& ortho_world2d(Mat4x4<T>& m, const T &width, const T &height)
{
	m.mat[0]	= 2.0 / width;
	m.mat[5]	= 2.0 / height;
	m.mat[10]	= -1.0;
	m.mat[12]	= m.mat[13]	= m.mat[14]	= 0.0;
	m.mat[15]	= 1.0;
	return m;
}

template<typename T> Mat4x4<T> ortho_world2d(const T &width, const T &height)
{	// build orthographical matrix
	Mat4x4<T> m;
	m.mat[1] = m.mat[2] = m.mat[3]
	= m.mat[4] = m.mat[6] = m.mat[7]
	= m.mat[8] = m.mat[9] = m.mat[11] = 0;
	return ortho_world2d<T>(m, width, height);
}

template<typename T> Mat4x4<T>& ortho_screen2d(Mat4x4<T>& m, const T &width, const T &height)
{
	m.mat[0]	= -2 / width;
	m.mat[5]	= -2 / -height;
	m.mat[10]	= m.mat[12] = m.mat[15] = 1;
	m.mat[13]	= -1;
	m.mat[14]	= 0;
	return m;
}

template<typename T> Mat4x4<T> ortho_screen2d(const T &width, const T &height)
{	// build orthographical matrix
	Mat4x4<T> m;
	m.mat[1] = m.mat[2] = m.mat[3]
	= m.mat[4] = m.mat[6] = m.mat[7]
	= m.mat[8] = m.mat[9] = m.mat[11] = 0;
	return ortho<T>(m, 0.0, width, height, 0.0, -1.0, 1.0);
}

template<typename T> Vec2<T> tranform(const Vec2<T> &p, const Mat4x4<T> &m)
{
	return Vec2<T>(p.x * m.mat[Xx] + p.y * m.mat[Xy] + m.mat[Wz],
					p.x * m.mat[Yx] + p.y * m.mat[Yy] + m.mat[Xw]);
}
